<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Transitional//EN" "http://www.w3.org/TR/xhtml1/DTD/xhtml1-transitional.dtd">
<html xmlns="http://www.w3.org/1999/xhtml">
<head>
<meta http-equiv="Content-Type" content="text/xhtml;charset=UTF-8"/>
<meta http-equiv="X-UA-Compatible" content="IE=9"/>
<title>PhysiTrack: D:/physi-tracklast/01/Android/PhysiTrack/src/process/KalmanFilter.java Source File</title>

<link href="tabs.css" rel="stylesheet" type="text/css"/>
<link href="doxygen.css" rel="stylesheet" type="text/css" />

<link href="search/search.css" rel="stylesheet" type="text/css"/>
<script type="text/javascript" src="jquery.js"></script>
<script type="text/javascript" src="search/search.js"></script>
<script type="text/javascript">
  $(document).ready(function() { searchBox.OnSelectItem(0); });
</script>

</head>
<body>
<div id="top"><!-- do not remove this div! -->


<div id="titlearea">
<table cellspacing="0" cellpadding="0">
 <tbody>
 <tr style="height: 56px;">
  
  
  <td style="padding-left: 0.5em;">
   <div id="projectname">PhysiTrack
   
   </div>
   
  </td>
  
  
  
 </tr>
 </tbody>
</table>
</div>

<!-- Generated by Doxygen 1.8.0 -->
<script type="text/javascript">
var searchBox = new SearchBox("searchBox", "search",false,'Search');
</script>
  <div id="navrow1" class="tabs">
    <ul class="tablist">
      <li><a href="index.html"><span>Main&#160;Page</span></a></li>
      <li><a href="namespaces.html"><span>Packages</span></a></li>
      <li><a href="annotated.html"><span>Classes</span></a></li>
      <li class="current"><a href="files.html"><span>Files</span></a></li>
      <li>
        <div id="MSearchBox" class="MSearchBoxInactive">
        <span class="left">
          <img id="MSearchSelect" src="search/mag_sel.png"
               onmouseover="return searchBox.OnSearchSelectShow()"
               onmouseout="return searchBox.OnSearchSelectHide()"
               alt=""/>
          <input type="text" id="MSearchField" value="Search" accesskey="S"
               onfocus="searchBox.OnSearchFieldFocus(true)" 
               onblur="searchBox.OnSearchFieldFocus(false)" 
               onkeyup="searchBox.OnSearchFieldChange(event)"/>
          </span><span class="right">
            <a id="MSearchClose" href="javascript:searchBox.CloseResultsWindow()"><img id="MSearchCloseImg" border="0" src="search/close.png" alt=""/></a>
          </span>
        </div>
      </li>
    </ul>
  </div>
  <div id="navrow2" class="tabs2">
    <ul class="tablist">
      <li><a href="files.html"><span>File&#160;List</span></a></li>
    </ul>
  </div>
</div>
<!-- window showing the filter options -->
<div id="MSearchSelectWindow"
     onmouseover="return searchBox.OnSearchSelectShow()"
     onmouseout="return searchBox.OnSearchSelectHide()"
     onkeydown="return searchBox.OnSearchSelectKey(event)">
<a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(0)"><span class="SelectionMark">&#160;</span>All</a><a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(1)"><span class="SelectionMark">&#160;</span>Classes</a><a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(2)"><span class="SelectionMark">&#160;</span>Namespaces</a><a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(3)"><span class="SelectionMark">&#160;</span>Files</a><a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(4)"><span class="SelectionMark">&#160;</span>Functions</a><a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(5)"><span class="SelectionMark">&#160;</span>Variables</a></div>

<!-- iframe showing the search results (closed by default) -->
<div id="MSearchResultsWindow">
<iframe src="javascript:void(0)" frameborder="0" 
        name="MSearchResults" id="MSearchResults">
</iframe>
</div>

<div class="header">
  <div class="headertitle">
<div class="title">D:/physi-tracklast/01/Android/PhysiTrack/src/process/KalmanFilter.java</div>  </div>
</div><!--header-->
<div class="contents">
<a href="_kalman_filter_8java.html">Go to the documentation of this file.</a><div class="fragment"><pre class="fragment"><a name="l00001"></a>00001 
<a name="l00002"></a>00002 <span class="keyword">package </span>process;
<a name="l00003"></a>00003 
<a name="l00004"></a>00004 <span class="keyword">import</span> Jama.Matrix;
<a name="l00005"></a>00005 
<a name="l00006"></a>00006 
<a name="l00008"></a><a class="code" href="classprocess_1_1_kalman_filter.html">00008</a> <span class="keyword">public</span> <span class="keyword">class </span><a class="code" href="classprocess_1_1_kalman_filter.html">KalmanFilter</a> {
<a name="l00009"></a>00009 
<a name="l00010"></a><a class="code" href="classprocess_1_1_kalman_filter.html#ac7905b43ff50eacc1e67da2e95c6e39d">00010</a>         <span class="keyword">private</span> Matrix <a class="code" href="classprocess_1_1_kalman_filter.html#ac7905b43ff50eacc1e67da2e95c6e39d">x</a>;
<a name="l00011"></a><a class="code" href="classprocess_1_1_kalman_filter.html#abb9c26b45f8de70c11d745a6b35648ee">00011</a>         <span class="keyword">private</span> Matrix <a class="code" href="classprocess_1_1_kalman_filter.html#abb9c26b45f8de70c11d745a6b35648ee">F</a>;
<a name="l00012"></a><a class="code" href="classprocess_1_1_kalman_filter.html#a666831f559eee38823b753ee088ed66e">00012</a>         <span class="keyword">private</span> Matrix <a class="code" href="classprocess_1_1_kalman_filter.html#a666831f559eee38823b753ee088ed66e">P</a>;
<a name="l00013"></a><a class="code" href="classprocess_1_1_kalman_filter.html#ac507680313a15a1f6e5b0dc262bc8b63">00013</a>         <span class="keyword">private</span> Matrix <a class="code" href="classprocess_1_1_kalman_filter.html#ac507680313a15a1f6e5b0dc262bc8b63">H</a>;
<a name="l00014"></a><a class="code" href="classprocess_1_1_kalman_filter.html#a57b4c3049e249c978ea874ceccf1a62c">00014</a>         <span class="keyword">private</span> Matrix <a class="code" href="classprocess_1_1_kalman_filter.html#a57b4c3049e249c978ea874ceccf1a62c">R</a>; <span class="comment">// sigmaM</span>
<a name="l00015"></a><a class="code" href="classprocess_1_1_kalman_filter.html#ab918483d9f212982291ad6c3a01a60ab">00015</a>         <span class="keyword">private</span> Matrix <a class="code" href="classprocess_1_1_kalman_filter.html#ab918483d9f212982291ad6c3a01a60ab">I</a>;
<a name="l00016"></a><a class="code" href="classprocess_1_1_kalman_filter.html#a2cfd1d6a3c54887f9b5d8cc429353d55">00016</a>         <span class="keyword">private</span> Matrix <a class="code" href="classprocess_1_1_kalman_filter.html#a2cfd1d6a3c54887f9b5d8cc429353d55">W</a>; <span class="comment">// sigmaW</span>
<a name="l00017"></a>00017         
<a name="l00018"></a>00018         <span class="comment">// ! setter function for F</span>
<a name="l00019"></a>00019         <span class="comment">/*</span>
<a name="l00020"></a>00020 <span class="comment">         * ! \param double</span>
<a name="l00021"></a>00021 <span class="comment">         */</span>
<a name="l00022"></a><a class="code" href="classprocess_1_1_kalman_filter.html#ac94c85a6084b26877bf46e559d65c054">00022</a>         <span class="keyword">private</span> <span class="keywordtype">void</span> <a class="code" href="classprocess_1_1_kalman_filter.html#ac94c85a6084b26877bf46e559d65c054">setF</a>(<span class="keywordtype">double</span> t) {
<a name="l00023"></a>00023                 <a class="code" href="classprocess_1_1_kalman_filter.html#abb9c26b45f8de70c11d745a6b35648ee">F</a>.set(0, 0, 1);
<a name="l00024"></a>00024                 <a class="code" href="classprocess_1_1_kalman_filter.html#abb9c26b45f8de70c11d745a6b35648ee">F</a>.set(1, 1, 1);
<a name="l00025"></a>00025                 <a class="code" href="classprocess_1_1_kalman_filter.html#abb9c26b45f8de70c11d745a6b35648ee">F</a>.set(2, 2, 1);
<a name="l00026"></a>00026                 <a class="code" href="classprocess_1_1_kalman_filter.html#abb9c26b45f8de70c11d745a6b35648ee">F</a>.set(0, 1, t);
<a name="l00027"></a>00027                 <a class="code" href="classprocess_1_1_kalman_filter.html#abb9c26b45f8de70c11d745a6b35648ee">F</a>.set(0, 2, t * t / 2);
<a name="l00028"></a>00028                 <a class="code" href="classprocess_1_1_kalman_filter.html#abb9c26b45f8de70c11d745a6b35648ee">F</a>.set(1, 2, t);
<a name="l00029"></a>00029         }
<a name="l00030"></a>00030 
<a name="l00031"></a>00031         <span class="comment">// ! Konstruktor</span>
<a name="l00032"></a>00032         <span class="comment">/*</span>
<a name="l00033"></a>00033 <span class="comment">         * ! \param double</span>
<a name="l00034"></a>00034 <span class="comment">         * ! \param double</span>
<a name="l00035"></a>00035 <span class="comment">         */</span>     
<a name="l00036"></a><a class="code" href="classprocess_1_1_kalman_filter.html#afaa2a948d5f986edb097599766c22566">00036</a>         <span class="keyword">public</span> <a class="code" href="classprocess_1_1_kalman_filter.html#afaa2a948d5f986edb097599766c22566">KalmanFilter</a>(<span class="keywordtype">double</span> sM, <span class="keywordtype">double</span> sW) {
<a name="l00037"></a>00037                 <a class="code" href="classprocess_1_1_kalman_filter.html#ac7905b43ff50eacc1e67da2e95c6e39d">x</a> = <span class="keyword">new</span> Matrix(<span class="keyword">new</span> <span class="keywordtype">double</span>[3][1]);
<a name="l00038"></a>00038                 <span class="keywordtype">double</span>[][] p = <span class="keyword">new</span> <span class="keywordtype">double</span>[3][3];
<a name="l00039"></a>00039                 p[0][0] = p[1][1] = p[2][2] = 1000;
<a name="l00040"></a>00040                 <a class="code" href="classprocess_1_1_kalman_filter.html#a666831f559eee38823b753ee088ed66e">P</a> = <span class="keyword">new</span> Matrix(p);
<a name="l00041"></a>00041                 <a class="code" href="classprocess_1_1_kalman_filter.html#abb9c26b45f8de70c11d745a6b35648ee">F</a> = <span class="keyword">new</span> Matrix(<span class="keyword">new</span> <span class="keywordtype">double</span>[3][3]);
<a name="l00042"></a>00042                 <a class="code" href="classprocess_1_1_kalman_filter.html#ac507680313a15a1f6e5b0dc262bc8b63">H</a> = <span class="keyword">new</span> Matrix(<span class="keyword">new</span> <span class="keywordtype">double</span>[1][3]);
<a name="l00043"></a>00043                 <a class="code" href="classprocess_1_1_kalman_filter.html#ac507680313a15a1f6e5b0dc262bc8b63">H</a>.set(0, 0, 1);
<a name="l00044"></a>00044                 <a class="code" href="classprocess_1_1_kalman_filter.html#a57b4c3049e249c978ea874ceccf1a62c">R</a> = <span class="keyword">new</span> Matrix(<span class="keyword">new</span> <span class="keywordtype">double</span>[1][1]);
<a name="l00045"></a>00045                 <a class="code" href="classprocess_1_1_kalman_filter.html#a57b4c3049e249c978ea874ceccf1a62c">R</a>.set(0, 0, sM);
<a name="l00046"></a>00046                 <a class="code" href="classprocess_1_1_kalman_filter.html#ab918483d9f212982291ad6c3a01a60ab">I</a> = Matrix.identity(3, 3);
<a name="l00047"></a>00047                 <a class="code" href="classprocess_1_1_kalman_filter.html#a2cfd1d6a3c54887f9b5d8cc429353d55">W</a> = (Matrix.identity(3, 3)).times(sW);
<a name="l00048"></a>00048 
<a name="l00049"></a>00049         }
<a name="l00050"></a>00050         <span class="comment">// ! the Kalman filter process</span>
<a name="l00051"></a>00051         <span class="comment">/*</span>
<a name="l00052"></a>00052 <span class="comment">         * ! \param double</span>
<a name="l00053"></a>00053 <span class="comment">         * ! \param double</span>
<a name="l00054"></a>00054 <span class="comment">         * ! \return double array</span>
<a name="l00055"></a>00055 <span class="comment">         */</span>
<a name="l00056"></a><a class="code" href="classprocess_1_1_kalman_filter.html#a1c1253df5a2f8d3a8707484f2eeda982">00056</a>         <span class="keyword">public</span> <span class="keywordtype">double</span>[] <a class="code" href="classprocess_1_1_kalman_filter.html#a1c1253df5a2f8d3a8707484f2eeda982">filter</a>(<span class="keywordtype">double</span> time, <span class="keywordtype">double</span> px) {
<a name="l00057"></a>00057                 <span class="comment">// 1predict</span>
<a name="l00058"></a>00058                 <a class="code" href="classprocess_1_1_kalman_filter.html#ac94c85a6084b26877bf46e559d65c054">setF</a>(time);
<a name="l00059"></a>00059                 <a class="code" href="classprocess_1_1_kalman_filter.html#ac7905b43ff50eacc1e67da2e95c6e39d">x</a> = <a class="code" href="classprocess_1_1_kalman_filter.html#abb9c26b45f8de70c11d745a6b35648ee">F</a>.times(<a class="code" href="classprocess_1_1_kalman_filter.html#ac7905b43ff50eacc1e67da2e95c6e39d">x</a>);
<a name="l00060"></a>00060                 <a class="code" href="classprocess_1_1_kalman_filter.html#a666831f559eee38823b753ee088ed66e">P</a> = ((<a class="code" href="classprocess_1_1_kalman_filter.html#abb9c26b45f8de70c11d745a6b35648ee">F</a>.times(<a class="code" href="classprocess_1_1_kalman_filter.html#a666831f559eee38823b753ee088ed66e">P</a>)).times(<a class="code" href="classprocess_1_1_kalman_filter.html#abb9c26b45f8de70c11d745a6b35648ee">F</a>.transpose())).plus(<a class="code" href="classprocess_1_1_kalman_filter.html#a2cfd1d6a3c54887f9b5d8cc429353d55">W</a>);
<a name="l00061"></a>00061 
<a name="l00062"></a>00062                 <span class="comment">// 2update</span>
<a name="l00063"></a>00063                 <span class="keywordtype">double</span> xy = px - <a class="code" href="classprocess_1_1_kalman_filter.html#ac7905b43ff50eacc1e67da2e95c6e39d">x</a>.get(0, 0);
<a name="l00064"></a>00064                 <span class="keywordtype">double</span> S = <a class="code" href="classprocess_1_1_kalman_filter.html#a666831f559eee38823b753ee088ed66e">P</a>.get(0, 0) + <a class="code" href="classprocess_1_1_kalman_filter.html#a57b4c3049e249c978ea874ceccf1a62c">R</a>.get(0, 0);
<a name="l00065"></a>00065                 Matrix K = <span class="keyword">new</span> Matrix(<span class="keyword">new</span> <span class="keywordtype">double</span>[3][1]);
<a name="l00066"></a>00066                 K = (<a class="code" href="classprocess_1_1_kalman_filter.html#a666831f559eee38823b753ee088ed66e">P</a>.getMatrix(0, 2, 0, 0).times(1 / S));
<a name="l00067"></a>00067                 <a class="code" href="classprocess_1_1_kalman_filter.html#ac7905b43ff50eacc1e67da2e95c6e39d">x</a> = <a class="code" href="classprocess_1_1_kalman_filter.html#ac7905b43ff50eacc1e67da2e95c6e39d">x</a>.plus(K.times(xy));
<a name="l00068"></a>00068                 <a class="code" href="classprocess_1_1_kalman_filter.html#a666831f559eee38823b753ee088ed66e">P</a> = (<a class="code" href="classprocess_1_1_kalman_filter.html#ab918483d9f212982291ad6c3a01a60ab">I</a>.minus(K.times(<a class="code" href="classprocess_1_1_kalman_filter.html#ac507680313a15a1f6e5b0dc262bc8b63">H</a>))).times(<a class="code" href="classprocess_1_1_kalman_filter.html#a666831f559eee38823b753ee088ed66e">P</a>);
<a name="l00069"></a>00069                 <span class="keywordtype">double</span>[] result = <span class="keyword">new</span> <span class="keywordtype">double</span>[3];
<a name="l00070"></a>00070                 result[0] = <a class="code" href="classprocess_1_1_kalman_filter.html#ac7905b43ff50eacc1e67da2e95c6e39d">x</a>.get(0, 0);
<a name="l00071"></a>00071                 result[1] = <a class="code" href="classprocess_1_1_kalman_filter.html#ac7905b43ff50eacc1e67da2e95c6e39d">x</a>.get(1, 0);
<a name="l00072"></a>00072                 result[2] = <a class="code" href="classprocess_1_1_kalman_filter.html#ac7905b43ff50eacc1e67da2e95c6e39d">x</a>.get(2, 0);
<a name="l00073"></a>00073                 <span class="keywordflow">return</span> result;
<a name="l00074"></a>00074         }
<a name="l00075"></a>00075 }
</pre></div></div><!-- contents -->


<hr class="footer"/><address class="footer"><small>
Generated on Fri Jun 8 2012 00:07:22 for PhysiTrack by &#160;<a href="http://www.doxygen.org/index.html">
<img class="footer" src="doxygen.png" alt="doxygen"/>
</a> 1.8.0
</small></address>

</body>
</html>
